#include "TaskGps.hpp"
#include "../../Util/BridgeIOConfig.hpp"
#include "cybertron/glm/quat.hpp"
#include "cybertron/glm/mat4.hpp"
CTaskGps::CTaskGps() {
	auto hostVehicleConfig = BridgeConfig::getInstance().getHostVehicleConfig();
	for (auto item = hostVehicleConfig.begin(); item != hostVehicleConfig.end(); item++) {
		IMUNoiseMakerMap::iterator it = mIMUNoiseMakerMap.find(item->first);
		const auto& sensorConfig = BridgeConfig::getInstance().sensorGpsImuConfig(item->first);
		if (it == mIMUNoiseMakerMap.end()) {
			const auto& sensorConfig = BridgeConfig::getInstance().sensorGpsImuConfig(item->first);
			mIMUNoiseMakerMap[item->first] = std::make_shared<IMUNoiseMaker>(sensorConfig);
		}
	}
	mOdometer = -1.0f;
	mLastPos = cybertron::vec3(0.f);
	mCommandID = Bridge::EHotAreaGPSData;
}
CTaskGps::~CTaskGps() {

}

void CTaskGps::ActorToDo(int hostVehicleId, int frame, int ms, int msModNs, TrafficActorHotArea* pHotArea, TrafficActor* pActor, int index) {

	/*if (!mIMUNoiseMaker) {
	}*/
	vec3 pos = pActor->getWorldPosition();
	vec3 rot = pActor->getWorldRotation();
	if (mOdometer < -1e-6) {
		mOdometer = 0;
	}
	else {
		mOdometer += cybertron::length(pos - mLastPos);
	}
	mLastPos = pos;
	HotArea::MainVehiclePrivateData MainVehiclePrivateData;

	std::string hostVehicle = std::to_string(hostVehicleId);
	if (pActor->getCustomProperties(MainVehiclePrivateData))
	{
		mLastGpsInfo.engineRpm = MainVehiclePrivateData.rpm();
		mLastGpsInfo.steering = MainVehiclePrivateData.steering();
		mLastGpsInfo.throttle = MainVehiclePrivateData.throttle();
		mLastGpsInfo.brake = MainVehiclePrivateData.brake();
		mLastGpsInfo.steering = MainVehiclePrivateData.steering();
		mLastGpsInfo.gear = MainVehiclePrivateData.gear();
		mLastGpsInfo.angVelX = MainVehiclePrivateData.angular_velocity_x();
		mLastGpsInfo.angVelY = MainVehiclePrivateData.angular_velocity_y();
		mLastGpsInfo.angVelZ = MainVehiclePrivateData.angular_velocity_z();
		mLastGpsInfo.wheelSpeedFL = MainVehiclePrivateData.wheel_speed_fl();
		mLastGpsInfo.wheelSpeedFR = MainVehiclePrivateData.wheel_speed_fr();
		mLastGpsInfo.wheelSpeedRL = MainVehiclePrivateData.wheel_speed_rl();
		mLastGpsInfo.wheelSpeedRR = MainVehiclePrivateData.wheel_speed_rr();

		vec3 Vel;
		Vel.x = MainVehiclePrivateData.linear_velocity_x();
		Vel.y = MainVehiclePrivateData.linear_velocity_y();
		Vel.z = MainVehiclePrivateData.linear_velocity_z();


		vec3 Accel;
		Accel.x = MainVehiclePrivateData.linear_acceleration_x();
		Accel.y = MainVehiclePrivateData.linear_acceleration_y();
		Accel.z = MainVehiclePrivateData.linear_acceleration_z();

		vec3 AngVel;
		AngVel.x = MainVehiclePrivateData.angular_velocity_x();
		AngVel.y = MainVehiclePrivateData.angular_velocity_y();
		AngVel.z = MainVehiclePrivateData.angular_velocity_z();

		const auto& sensorConfig = BridgeConfig::getInstance().sensorGpsImuConfig(hostVehicle);

		if (!mIMUNoiseMakerMap.empty()) {
			vec3 posWithNoise;
			vec3 rotWithNoise;
			vec3 velWithNoise;
			vec3 localAccWithNoise;
			vec3 angVelWithNoise;
			mIMUNoiseMakerMap[hostVehicle]->update(pos, rot, frame);
			vec3 posOffset = pos + quat(rot) * sensorConfig.Position;
			posWithNoise= posOffset + mIMUNoiseMakerMap[hostVehicle]->positionNoise();
			rotWithNoise= rot + mIMUNoiseMakerMap[hostVehicle]->attitudeNoise();
			velWithNoise = Vel + mIMUNoiseMakerMap[hostVehicle]->velocityNoise();
			localAccWithNoise = inverse(mat4_cast(rot)) * Accel + mIMUNoiseMakerMap[hostVehicle]->accelerationNoise();
			angVelWithNoise = AngVel + mIMUNoiseMakerMap[hostVehicle]->angularVelocityNoise();

			mLastGpsInfo.posX = posWithNoise.x;
			mLastGpsInfo.posY = posWithNoise.y;
			mLastGpsInfo.posZ = posWithNoise.z;

			mLastGpsInfo.oriX = rotWithNoise.x;
			mLastGpsInfo.oriY = rotWithNoise.y;
			mLastGpsInfo.oriZ = rotWithNoise.z;

			mLastGpsInfo.velX = velWithNoise.x;
			mLastGpsInfo.velY = velWithNoise.y;
			mLastGpsInfo.velZ = velWithNoise.z;

			mLastGpsInfo.accelX = localAccWithNoise.x;
			mLastGpsInfo.accelY = localAccWithNoise.y;
			mLastGpsInfo.accelZ = localAccWithNoise.z;

			mLastGpsInfo.angVelX = angVelWithNoise.x;
			mLastGpsInfo.angVelY = angVelWithNoise.y;
			mLastGpsInfo.angVelZ = angVelWithNoise.z;
		}
		else {
			mLastGpsInfo.posX = pos.x;
			mLastGpsInfo.posY = pos.y;
			mLastGpsInfo.posZ = pos.z;

			mLastGpsInfo.oriX = rot.x;
			mLastGpsInfo.oriY = rot.y;
			mLastGpsInfo.oriZ = rot.z;

			mLastGpsInfo.velX = Vel.x;
			mLastGpsInfo.velY = Vel.y;
			mLastGpsInfo.velZ = Vel.z;

			mLastGpsInfo.accelX = Accel.x;
			mLastGpsInfo.accelY = Accel.y;
			mLastGpsInfo.accelZ = Accel.z;

			mLastGpsInfo.angVelX = AngVel.x;
			mLastGpsInfo.angVelY = AngVel.y;
			mLastGpsInfo.angVelZ = AngVel.z;
		}
		



		mLastGpsInfo.odometer = mOdometer;

		mLastGpsInfo.extraStateSize = MainVehiclePrivateData.extrastates_size();
		for (int i = 0; i < MainVehiclePrivateData.extrastates_size(); i++) {
			mLastGpsInfo.extraStates[i] = (MainVehiclePrivateData.extrastates(i));
		}
		mLastGPSDataMap[std::to_string(hostVehicleId)] = mLastGpsInfo;
	}
}
void CTaskGps::HotAreaEnter() {
	memset(&mLastGpsInfo,0,sizeof(SimOne_Data_Gps));
	mLastGPSDataMap.clear();
}
void CTaskGps::HotAreaLeft(int frame, int ms, int msModNs, TrafficActorHotArea* pHotArea) {
}
char* CTaskGps::GetResult(std::string hostVehicleId) {
	for (auto item = mLastGPSDataMap.begin(); item != mLastGPSDataMap.end(); item++) {
		if (item->first == hostVehicleId) {
			return (char*)&item->second;
		}
	}
	return nullptr;
	//return (char*)&mLastGpsInfo;
}

int CTaskGps::GetHotAreaDataLength(std::string hostVehicleId) {
	return sizeof(mLastGpsInfo);
}

void CTaskGps::Clear() {
	mResultMessage.clear();
}
